/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package zachowania;

import java.util.ArrayList;
import java.util.Iterator;
import obiekty.Mur;
import obiekty.ObiektRuchomy;
import obiekty.ObiektStaly;
import prostagra.Mapa;
import utils.Vector2D;

/**
 * Klasa wyliczając siłę sterującą na bazie ustawioych w niej zachowań sterujących
 * Zmienne zawierają wagi danych zachowań
 * @author m
 */
public class ZachowaniaSterujace
{

    private boolean seek = false,  flee = false,  arrive = false,  pathfollow = false,  wander = false,  cohesion = false,  separation = false,  aligment = false,  obstacleAvoidance = false;
    double prWallAvoidance = 0.9;
    double prObstacleAvoidance = 1.0;
    double prWander = 0.8;
    double prArrive = 0.8;
    double prSeek = 0.8;
    double prFlee = 0.8;
    double prPathFollow = 0.8;
    double prSeparation = 0.8;
    double prAlignment = 0.8;
    double prCohesion = 0.6;
/**
 * Fukcja wylicza siłę sterującą
 * @param ObiektRuchomy o - obiekt dla którego wyliczana jest siła
 * @param Vector2D cel - ewentualny punkt docelowy zachowania
 * @return wyliczona siła sterująća
 */
    public Vector2D calculate(ObiektRuchomy o, Vector2D cel)
    {
        Vector2D steeringForce = new Vector2D(0, 0);
        if (obstacleAvoidance)
        {
            Vector2D n = dodajSile(o, steeringForce, obstacleAvoidance(o).multiple(0.3).multiple(prObstacleAvoidance));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }
        if (arrive)
        {
            Vector2D n = dodajSile(o, steeringForce, arrive(o, cel).multiple(prArrive));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }

        }
        if (wander)
        {
            Vector2D n = dodajSile(o, steeringForce, steeringForce.add(wander(o)).multiple(prWander));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }
        if (seek)
        {
            Vector2D n = dodajSile(o, steeringForce, steeringForce.add(seek(o, cel)).multiple(prSeek));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }
        if (flee)
        {
            Vector2D n = dodajSile(o, steeringForce, steeringForce.add(flee(o, cel)).multiple(prFlee));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }
        if (pathfollow)
        {
            Vector2D n = dodajSile(o, steeringForce, steeringForce.add(pathfollow(o)).multiple(prPathFollow));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }



        if (separation)
        {
            Vector2D n = dodajSile(o, steeringForce, steeringForce.add(separation(o)).multiple(prSeparation));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }
        if (aligment)
        {
            Vector2D n = dodajSile(o, steeringForce, steeringForce.add(aligment(o)).multiple(prAlignment));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }
        if (cohesion)
        {
            Vector2D n = dodajSile(o, steeringForce, steeringForce.add(cohesion(o)).multiple(prCohesion));
            if (n.length() != 0)
            {
                steeringForce = n;
            }
            if (steeringForce.length() >= o.getMaxSila())
            {
                return steeringForce;
            }
        }

        return steeringForce.cutTo(o.getMaxSila());
    }

    private Vector2D dodajSile(ObiektRuchomy o, Vector2D sila, Vector2D dodawanaSila)
    {
        //calculate how much steering force remains to be used by this vehicle
        double maxDoDodania = o.getMaxSila() - sila.length();
        if (maxDoDodania <= 0.0)
        {
            return new Vector2D(0, 0);
        }
        return sila.add(dodawanaSila.cutTo(maxDoDodania));
    }

    public void seekOn()
    {
        seek = true;
    }

    public void seekOff()
    {
        seek = false;
    }

    public void fleeOn()
    {
        flee = true;
    }

    public void fleeOff()
    {
        flee = false;
    }

    public void arriveOn()
    {
        arrive = true;
    }

    public void arriveOff()
    {
        arrive = false;
    }

    public void pathfollowOn()
    {
        pathfollow = true;
    }

    public void pathfollowOff()
    {
        pathfollow = false;
    }

    public void wanderOn()
    {
        wander = true;
    }

    public void wanderOff()
    {
        wander = false;
    }

    public void separationOn()
    {
        separation = true;
    }

    public void separationOff()
    {
        separation = false;
    }

    public void aligmentOn()
    {
        aligment = true;
    }

    public void aligmentOff()
    {
        aligment = false;
    }

    public void cohesionOn()
    {
        cohesion = true;
    }

    public void cohesionOff()
    {
        cohesion = false;
    }

    public void obstacleAvoidanceOn()
    {
        obstacleAvoidance = true;
    }

    public void obstacleAvoidanceOff()
    {
        obstacleAvoidance = false;
    }
    // STEROWANIA
    private Vector2D arrive(ObiektRuchomy o, Vector2D cel)
    {
        cel.x = (int) cel.x;
        cel.y = (int) cel.y;
        o.getPozycja().x = (int) o.getPozycja().x;
        o.getPozycja().y = (int) o.getPozycja().y;

        Vector2D toTarget = cel.subtract(o.getPozycja());
        double decelRate = 1.0;
        double dist = toTarget.length();
        //System.out.println(dist);

        if (dist == 0)
        {
            return new Vector2D(0, 0);
        }
        if (dist < 10.0)
        {
            return toTarget.subtract(o.getPredkosc());
        } else if (dist < 30.0)
        {
            decelRate = 100.0;
        } else if (dist < 50.0)
        {
            decelRate = 20.0;
        } else if (dist < 75.0)
        {
            decelRate = 5.5;
        } else if (dist < 100.0)
        {
            decelRate = 1.0;
        } else if (dist >= 100.0)
        {
            decelRate = 0.2;
        }

        double speed = dist / decelRate;
        speed = Math.min(speed, o.getMaxPredkosc());
        Vector2D desiredVelocity = toTarget.multiple(speed).divide(dist);

        return desiredVelocity.subtract(o.getPredkosc()).cutTo(o.getMaxPredkosc());
    }
    Vector2D targetCircle = new Vector2D(0, 0);
    double wanderRadius = 10.0;
    double wanderDistance = 2.0;
    double randomDisplacement = 1.2;

    private Vector2D wander(ObiektRuchomy o)
    {
        Vector2D desiredVelocity = new Vector2D(0, 0);
        //przesuwamy okrag celu o malutki wektor pomnozony przez randomDisplacement
        targetCircle = targetCircle.add(new Vector2D(Math.random() * 2.0 - 1.0, Math.random() * 2.0 - 1.0).multiple(randomDisplacement));

        //normalizujemy wektor polozenia okregu celu i mnozymy przez promien duzego okregu
        //w rezultacie rzutujemy okrag celu spowtorem na duzy okrag

        targetCircle = targetCircle.normalize().multiple(wanderRadius);

        desiredVelocity = targetCircle.add(o.getPredkosc().normalize().multiple(wanderDistance));
        //.multiple(self.mass).cutTo(self.max_force)
        return desiredVelocity.subtract(o.getPredkosc()).cutTo(o.getMaxPredkosc());
    }

    public void setWanderRadius(ObiektRuchomy o, double radius)
    {
        wanderRadius = radius;
        targetCircle = o.getPredkosc().normalize().multiple(radius / 2.0);
    }

    private Vector2D seek(ObiektRuchomy o, Vector2D cel)
    {
        Vector2D vectorToTarget = cel.subtract(o.getPozycja());
        if (vectorToTarget.length() != 0)
        {
            Vector2D desiredVelocity = ((vectorToTarget).normalize()).multiple(o.getMaxPredkosc());
            // F = ( (V1 - V2).cutTo(maxV) * mass * czasJednostkowy ).cutTo(maxF)
            return desiredVelocity.subtract(o.getPredkosc()).cutTo(o.getMaxPredkosc());
        } else
        {
            return new Vector2D(0.0, 0.0);
        }
    }

    private Vector2D flee(ObiektRuchomy o, Vector2D cel)
    {
        Vector2D vectorToTarget = o.getPozycja().subtract(cel);
        if (vectorToTarget.length() != 0)
        {
            Vector2D desiredVelocity = (vectorToTarget).normalize().multiple(o.getMaxPredkosc());
            return desiredVelocity.subtract(o.getPredkosc()).cutTo(o.getMaxPredkosc());
        } else
        {
            return new Vector2D(0.0, 0.0);
        }
    }
    int currentWPT = 0;
    ArrayList<Vector2D> path;

    public void setPath(ArrayList<Vector2D> path)
    {
        this.path = path;
    }

    private Vector2D pathfollow(ObiektRuchomy o)
    {
        Vector2D targetPos = path.get(currentWPT);
        Vector2D toTarget = targetPos.subtract(o.getPozycja());

        if (toTarget.length() < 10.0)
        {
            nextWPT();
        }
        return seek(o, targetPos);
    }

    private void nextWPT()
    {
        if (currentWPT + 1 < path.size())
        {
            currentWPT++;
        } else
        {
            currentWPT = 0;
        }
    }

    private Vector2D separation(ObiektRuchomy o)
    {

        Vector2D steeringForce = new Vector2D(0.0, 0.0);
        if (o.getGrupa() != null && o.getGrupa().getListaObiektow() != null)
        {
            for (int i = 0; i < o.getGrupa().getListaObiektow().size(); i++)
            {
                if (o.getGrupa().getListaObiektow().get(i).equals(o) == false)
                {
                    Vector2D toAgent = o.getPozycja().subtract(o.getGrupa().getListaObiektow().get(i).getPozycja());
                    if (toAgent.length() != 0)
                    {
                        toAgent = toAgent.normalize().divide(toAgent.length());
                        steeringForce = steeringForce.add(toAgent);
                    }
                }
            }
        }
        return steeringForce.cutTo(o.getMaxSila());
    }

    private Vector2D aligment(ObiektRuchomy o)
    {
        Vector2D sredniaOrientacja = new Vector2D(0.0, 0.0);

        if (o.getGrupa() != null && o.getGrupa().getListaObiektow() != null)
        {
            for (int i = 0; i < o.getGrupa().getListaObiektow().size(); i++)
            {
                if (o.getGrupa().getListaObiektow().get(i).equals(o) == false)
                {
                    sredniaOrientacja = sredniaOrientacja.add(o.getGrupa().getListaObiektow().get(i).getOrientacja());
                }
            }

            if (o.getGrupa().getListaObiektow().size() - 1 > 0)
            {
                sredniaOrientacja = sredniaOrientacja.divide(o.getGrupa().getListaObiektow().size() - 1);
                sredniaOrientacja = sredniaOrientacja.subtract(o.getOrientacja());
            }
        }
        o.setOrientacja(sredniaOrientacja.normalize());
        return sredniaOrientacja.cutTo(o.getMaxSila());
    }

    private Vector2D cohesion(ObiektRuchomy o)
    {
        Vector2D centerOfMass = new Vector2D(0.0, 0.0);
        Vector2D steeringForce = new Vector2D(0.0, 0.0);

        if (o.getGrupa() != null && o.getGrupa().getListaObiektow() != null)
        {
            for (int i = 0; i < o.getGrupa().getListaObiektow().size(); i++)
            {
                if (o.getGrupa().getListaObiektow().get(i).equals(o) == false)
                {
                    centerOfMass = centerOfMass.add(o.getGrupa().getListaObiektow().get(i).getPozycja());
                }
            }

            if (o.getGrupa().getListaObiektow().size() - 1 > 0)
            {
                centerOfMass = centerOfMass.divide(o.getGrupa().getListaObiektow().size() - 1);
                steeringForce = seek(o, centerOfMass).multiple(0.3);
            }
        }
        return steeringForce.cutTo(o.getMaxSila());
    }
    double minDetectionBoxLength = 60.0;
    Mapa mapa;

    Vector2D wallAvoidance(ObiektRuchomy o)
    {
        mapa = Mapa.getInstance();
        Vector2D steeringForce = new Vector2D(0, 0);


        return steeringForce;
    }

    Vector2D obstacleAvoidance(ObiektRuchomy o)
    {
        mapa = Mapa.getInstance();

        //wyznaczamy długość pudełka detekcji kolizji
        double m_dDBoxLength = minDetectionBoxLength +
                (o.getPredkosc().length() / o.getMaxPredkosc()) * minDetectionBoxLength;

        double kat = o.getOrientacja().angle(o.getPozycja());

        //this will keep track of the closest intersecting obstacle (CIB)
        ObiektStaly najblizszyObiekt = null;


        // najmniejsza odległość do obiektu stałego
        double DistToClosestIP = Double.MAX_VALUE;

        // pozycja najbliższego obiektu
        Vector2D LocalPosOfClosestObstacle = null;


        synchronized (mapa.getObiektyStale())
        {
            Iterator<ObiektStaly> itOb = mapa.getObiektyStale().iterator();
            while (itOb.hasNext())
            {

                ObiektStaly curOb = itOb.next();

                // odległość do obiektu stałego jest mniejsza bądź równa długości pudełka do detekcji kolizji
                // to analizujemy dalej
                if (curOb.getPozycja().subtract(o.getPozycja()).length() - o.getPromien() - curOb.getPromien() < m_dDBoxLength)
                {
                    // przesuwamy początek układu współrzędnych do punktu w którym znajduje się 
                    // poruszający się obiekt. Oś Y jest ustawiona w kierunku poruszania się obiektu
                    Vector2D LocalPos = curOb.getPozycja().subtract(o.getPozycja()).rotate(2.0 * Math.PI - kat);
                    // oX i oY określają położenie obiektu względem początku układu współrzędnych
                    double oY = -LocalPos.y;
                    double oX = -LocalPos.x;
                    /*
                     * Jeśli współrzędna y jest większa od 0 to obiekt stały jest przed poruszającym się obiektem
                     */
                    if (oY >= 0 && LocalPos.length() - o.getPromien() - curOb.getPromien() < m_dDBoxLength)
                    {

                        double ExpandedRadius = (curOb).getPromien() + o.getPromien();
                        /* 
                         * Jeśli współrzędna x jest mniejsza od sumy promieni dwóch obiektów: poruszającego się
                         * i stałego to jest potencjalna kolizja
                         */
                        if (Math.abs(oX) < ExpandedRadius)
                        {
                            /*
                             * jeżeli jesteśmy tutaj to oznacza to, że jeśli dalej poruszałby się tak
                             * obiekt to nastąpiłaby kolizja z innym obiektem. 
                             */

                            /*
                             * Obliczamy tylko odległość Y do obiektu stałego
                             */
                            double odl = Math.abs(oY - ExpandedRadius);


                            /*
                             * Jeśli ta odległość jest mniejsza od dotychczasowej najmniejszej odległości
                             * do najbliższego obiektu stałego to mamy nowy obiekt najbliższy, który trzeba ominąć
                             */
                            if (odl < DistToClosestIP)
                            {
                                DistToClosestIP = odl;

                                najblizszyObiekt = curOb;
                                LocalPosOfClosestObstacle = LocalPos;
                            }
                        }
                    }
                }
            }
        }


        // Obliczamy siłę sterującą, jeśli mamy najbliższy obiekt, z którym może nastąpić kolizja
        Vector2D SteeringForce = new Vector2D(0, 0);
        if (najblizszyObiekt != null)
        {

            double multiplier = 1.0 + (najblizszyObiekt.getPozycja().subtract(o.getPozycja()).length() - o.getPromien() - najblizszyObiekt.getPromien()) /
                    m_dDBoxLength;


            // siła odsuwająca od przeszkody
            SteeringForce.x = ((-LocalPosOfClosestObstacle.x) - (najblizszyObiekt.getPromien() + o.getPromien())) * 0.01 * multiplier;

            // siła hamująca
            double BrakingWeight = 0.005;
            SteeringForce.y = ((-LocalPosOfClosestObstacle.y) - najblizszyObiekt.getPromien() + o.getPromien()) * BrakingWeight;

            SteeringForce = SteeringForce.rotate(kat);
            SteeringForce = SteeringForce.cutTo(o.getMaxSila());

        }

        return SteeringForce;
    }
}
